/*
 * 文件名：bsp_ble.cpp
 * 功能描述：蓝牙低功耗(BLE)通信模块
 */

#include <Arduino.h>
#include <ArduinoJson.h>
#include "bsp_esp32.h"
#include "bsp_ble.h"
#include "bsp_sdcard.h"
#include "arduino_utils.h"
#include "bsp_calibration.h"
#include "hbot_motion.h"
#include "bsp_motor.h"
#include "ElectromagnetControl.h"
#include "hall_id.h"
#include "esp_task_wdt.h"
#include "BLEDevice.h"
#include "BLEUtils.h"
#include "BLEServer.h"
#include <hall_id.h>
#include "endstop.h"
#include "hall_id.h"

Bluetooth ble_g;
BLEServer *pServer; // 定义BLE服务器指针

BLECharacteristic *pCharac_json;         // 用于json命令传输
BLECharacteristic *pCharac_fileio_ascii; // 用于文件收发
BLECharacteristic *pCharac_fileio_hex;   // 用于文件收发

BLECharacteristic *pCharac_notify_msg; // 用于错误提示（主动nofity，App需关注）-广播id矩阵
BLECharacteristic *pCharac_state_msg;  // 用于广播状态矩阵

BLECharacteristic *pCharac_debug_cmd; // 用于debug命令，字符串格式
BLECharacteristic *pCharac_motor_cmd; // 用于电机单独控制(十六进制）
BLECharacteristic *pCharac_music_cmd; // 用于串口音频模块
BLECharacteristic *pCharac_rgb_cmd;   // 用于ws2812
BLECharacteristic *pCharacteristic = nullptr;

String cmdName;
String cmdMovelist;
///______________________________________________________________________________________________
//// 主回调函数声明

/**
 * @brief BLE服务主回调类
 *
 * 处理设备连接/断开事件：
 * - 设备连接时更新连接状态标志
 * - 设备断开后自动重启广播
 */
class BleMainServerCallbacks : public BLEServerCallbacks
{
    void onConnect(BLEServer *pServer)
    {
        if (print_key)
        {
            Serial.println("现在有设备接入~");
        }
        // send_print(finalArr);
        deviceConnected_g = true;
    };

    void onDisconnect(BLEServer *pServer)
    {
        if (print_key)
        {
            Serial.println("现在有设备断开连接~");
        }
        deviceConnected_g = false;
        // 在有设备接入后Advertising广播会被停止，所以要在设备断开连接时重新开启广播
        // 不然的话只有重启ESP32后才能重新搜索到
        delay(1000);                 // 给设备一点时间来重新启动广告
        pServer->startAdvertising(); // 该行效果同 BLEDevice::startAdvertising();
    }
};

/**
 * @brief JSON命令特征值回调处理
 *
 * 解析接收到的JSON格式指令：
 * 1. 运动控制指令（速度/音量设置）
 * 2. 棋局操作指令（开局/移动/悔棋）
 * 3. 系统控制指令（重启/校准）
 * @see Command 命令队列数据结构
 */
///______________________________________________________________________________________________
/// Json Service
String json_cmd = "json";
String current_rev_json_cmd = "";
class callback_json_cmd : public BLECharacteristicCallbacks
{
    void onWrite(BLECharacteristic *pCharacteristic) override
    {
        // 处理接收到的数据
        std::string rxValue = pCharacteristic->getValue();
        if (rxValue.length() > 0)
        {
            if (print_key)
            {
                Serial.printf("Received cmd: %s\n", rxValue.c_str());
            }
            String jsonString = String(rxValue.c_str());
            //​​创建 JSON 文档对象
            StaticJsonDocument<1024> doc; // 确保足够的内存
            // 解析 JSON 字符串，字符串转为JSON
            DeserializationError error = deserializeJson(doc, jsonString);

            if (error != DeserializationError::Ok)
            {
                if (print_key)
                {
                    Serial.printf("Invalid JSON: %d\n", error);
                }
                return;
            }
            if ((doc["cmd_name"].as<String>() == "settings"))
            {
                beep_set((int)doc["cmd_args"]["beep_volume"]);    // beep
                motor_speed_set(doc["cmd_args"]["motion_speed"]); // 速度
                speaker_set(doc["cmd_args"]["voice_control"]);    // 语音模块控制
                if (print_key)
                {
                    Serial.printf("设置");
                }

                isSyncing = true;
                StaticJsonDocument<512> response;
                response["cmd_name"] = "settings";
                response["status"] = "success";
                String responseStr;
                serializeJson(response, responseStr);
                pCharacteristic->setValue(responseStr.c_str());
                pCharacteristic->notify();
            }

            if ((doc["cmd_name"].as<String>() == "power"))
            {
                StaticJsonDocument<512> response;
                response["cmd_name"] = "power";
                response["status"] = batteryLevel_g;
                String responseStr;
                serializeJson(response, responseStr);
                pCharacteristic->setValue(responseStr.c_str());
                pCharacteristic->notify();
                //
            }

            // 处理设置速度的命令
            if ((doc["cmd_name"].as<String>() == "set_speed"))
            {

                int speed = doc["cmd_args"]["speed"].as<int>();
                Command cmd;
                cmd.cmd_name = "set_speed";
                cmdName = "set_speed";
                // 检查是否包含id字段并提取
                if (doc.containsKey("id"))
                {
                    cmd.id = String(doc["id"].as<const char *>());
                    if (print_key)
                    {
                        Serial.printf("接收到ID参数: %s\n", cmd.id.c_str());
                    }
                }
                else
                {
                    cmd.id = ""; // 如果没有提供id，设为空字符串
                }
                cmd.speed = speed;
                if (xQueueSend(commandQueue, &cmd, portMAX_DELAY) != pdPASS)
                {
                    Serial.println("命令队列已满，无法添加新命令");
                }
            }

            // 处理设置蜂鸣器音量的命令
            if ((doc["cmd_name"].as<String>() == "set_beep_volume"))
            {
                int input_beep_volume = doc["cmd_args"]["num"].as<int>();

                Command cmd;
                cmd.cmd_name = "set_beep_volume";
                cmdName = "set_beep_volume";
                cmd.input_beep_volume = input_beep_volume;
                // 检查是否包含id字段并提取
                if (doc.containsKey("id"))
                {
                    cmd.id = String(doc["id"].as<const char *>());
                    if (print_key)
                    {
                        Serial.printf("接收到ID参数: %s\n", cmd.id.c_str());
                    }
                }
                else
                {
                    cmd.id = ""; // 如果没有提供id，设为空字符串
                }
                if (xQueueSend(commandQueue, &cmd, portMAX_DELAY) != pdPASS)
                {
                    Serial.println("命令队列已满，无法添加新命令");
                }
            }

            // 处理设置语音模块状态的命令
            if ((doc["cmd_name"].as<String>() == "set_voice_control"))
            {
                int isOpen = doc["cmd_args"]["isOpen"].as<int>();
                Command cmd;
                cmd.cmd_name = "set_voice_control";
                cmd.isOpen = isOpen;
                if (xQueueSend(commandQueue, &cmd, portMAX_DELAY) != pdPASS)
                {
                    Serial.println("命令队列已满，无法添加新命令");
                }
            }
            // 根据不同的命令执行操作
            if ((doc["cmd_name"].as<String>() == "game_over"))
            {
                Command cmd;
                cmd.cmd_name = "game_over";
                calibrate();
                resetBoard();
                gameStarted = false;
                id_board = "";
                //                flushQueue(commandQueue);
                //                sendprint("game_over", "success", "", nullptr);
                //
                //                if (xQueueSend(commandQueue, &cmd, portMAX_DELAY) != pdPASS) {
                //                    Serial.println("命令队列已满，无法添加新命令");
                //                }
            }

            // 悔棋
            if (doc["cmd_name"].as<String>() == "undo_move")
            {
                Command cmd;
                cmd.cmd_name = "undo_move";
                cmd.number = doc["number"].as<int>();
                if (xQueueSend(commandQueue, &cmd, portMAX_DELAY) != pdPASS)
                {
                    Serial.println("命令队列已满，无法添加新命令");
                }
            }

            // 自动摆棋
            if (doc["cmd_name"].as<String>() == "auto_init")
            {
                Command cmd;
                cmd.cmd_name = "auto_init";
                if (xQueueSend(commandQueue, &cmd, portMAX_DELAY) != pdPASS)
                {
                    Serial.println("命令队列已满，无法添加新命令");
                }
            }

            // 处理不同命令的情况
            if (doc["cmd_name"].as<String>() == "start_new_game")
            {
                const char *play_as = doc["cmd_args"]["play_as"];
                Command cmd;
                cmd.cmd_name = "start_new_game";
                cmd.play_as = play_as;
                if (xQueueSend(commandQueue, &cmd, portMAX_DELAY) != pdPASS)
                {
                    Serial.println("命令队列已满，无法添加新命令");
                }
            }

            if (doc["cmd_name"].as<String>() == "start_game")
            {
                Command cmd;
                cmd.cmd_name = "start_game";
                cmdName = String("start_game");
                // 检查是否包含id字段并提取
                if (doc.containsKey("id"))
                {
                    cmd.id = String(doc["id"].as<const char *>());
                    if (print_key)
                    {
                        Serial.printf("接收到ID参数: %s\n", cmd.id.c_str());
                    }
                }
                else
                {
                    cmd.id = ""; // 如果没有提供id，设为空字符串
                }

                if (xQueueSend(commandQueue, &cmd, portMAX_DELAY) != pdPASS)
                {
                    Serial.println("命令队列已满，无法添加新命令");
                }
            }
            else if (doc["cmd_name"].as<String>() == "game_over")
            {
                Command cmd;
                cmd.cmd_name = "game_over";
                if (xQueueSend(commandQueue, &cmd, portMAX_DELAY) != pdPASS)
                {
                    Serial.println("命令队列已满，无法添加新命令");
                }
            }
            // 移动棋子命令，freemove
            else if (doc["cmd_name"].as<String>() == "free_move")
            {
                const char *move = doc["cmd_args"]["move"];
                Command cmd;
                cmd.cmd_name = String("free_move");

                cmdName = String("free_move");
                //取出来往哪走
                cmd.move = move;

                // 检查是否包含id字段并提取,id是这条消息的ID，用于给app通信
                if (doc.containsKey("id"))
                {
                    cmd.id = String(doc["id"].as<const char *>());
                    if (print_key)
                    {
                        Serial.printf("接收到ID参数: %s\n", cmd.id.c_str());
                    }
                }
                else
                {
                    cmd.id = ""; // 如果没有提供id，设为空字符串
                }

                if (xQueueSend(commandQueue, &cmd, portMAX_DELAY) != pdPASS)
                {
                    Serial.println("命令队列已满，无法添加新命令");
                }
            }


            //群移动
            if (doc["cmd_name"].as<String>() == "sog")
            {
                Command cmd;
                cmd.cmd_name = String("sog");
                cmdName = String("sog");
                if (doc["cmd_args"].containsKey("ml"))
                {

                    //move的起点坐标和终点坐标
                    cmd.move_list = doc["cmd_args"]["ml"].as<String>();
                    //备份
                    cmdMovelist = cmd.move_list;
                    Serial.println(cmd.move_list);
                
                }
                else
                {
                    cmd.move_list = ""; // 如果没有提供FEN字符串，设置为空
                }

                // 检查是否包含id字段并提取
                if (doc.containsKey("id"))
                {
                    cmd.id = String(doc["id"].as<const char *>());
                    if (print_key)
                    {
                        Serial.printf("接收到ID参数: %s\n", cmd.id.c_str());
                    }
                }
                else
                {
                    cmd.id = ""; // 如果没有提供id，设为空字符串
                }

                if (xQueueSend(commandQueue, &cmd, portMAX_DELAY) != pdPASS)
                {
                    Serial.println("命令队列已满，无法添加新命令");
                }
            }
            else if (doc["cmd_name"].as<String>() == "pathmove")
            {
                Command cmd;
                cmd.cmd_name = String("pathmove");
                cmdName = String("pathmove");
                if (doc["cmd_args"].containsKey("path"))
                {
                    cmd.path_move_list = doc["cmd_args"]["path"].as<String>();
                    Serial.println(cmd.path_move_list);
                }
                else
                {
                    cmd.path_move_list = ""; // 如果没有提供FEN字符串，设置为空
                }

                // 检查是否包含id字段并提取
                if (doc.containsKey("id"))
                {
                    cmd.id = String(doc["id"].as<const char *>());
                }
            }
            else if (doc["cmd_name"].as<String>() == "get_equipment_data")
            {
                Command cmd;
                cmd.cmd_name = String("get_equipment_data");
                cmdName = String("get_equipment_data");

                // 检查是否包含id字段并提取
                if (doc.containsKey("id"))
                {
                    cmd.id = String(doc["id"].as<const char *>());
                    if (print_key)
                    {
                        Serial.printf("接收到ID参数: %s\n", cmd.id.c_str());
                    }
                }
                else
                {
                    cmd.id = ""; // 如果没有提供id，设为空字符串
                }

                if (xQueueSend(commandQueue, &cmd, portMAX_DELAY) != pdPASS)
                {
                    Serial.println("命令队列已满，无法添加新命令");
                }
            }
            else if (doc["cmd_name"].as<String>() == "get_movement_position")
            {
                Command cmd;
                // 使用 String 构造函数确保深拷贝
                cmd.cmd_name = String("get_movement_position");
                cmdName = String("get_movement_position");
                // 检查是否包含id字段并提取
                if (doc.containsKey("id"))
                {
                    cmd.id = String(doc["id"].as<const char *>());
                    if (print_key)
                    {
                        Serial.printf("接收到ID参数: %s\n", cmd.id.c_str());
                    }
                }
                else
                {
                    cmd.id = ""; // 如果没有提供id，设为空字符串
                }

                // 打印诊断信息
                Serial.printf("添加命令到队列: '%s', ID: '%s'\n", cmd.cmd_name.c_str(), cmd.id.c_str());

                if (xQueueSend(commandQueue, &cmd, portMAX_DELAY) != pdPASS)
                {
                    Serial.println("命令队列已满，无法添加新命令");
                }
            }
            else if (doc["cmd_name"].as<String>() == "get_id_board")
            {
                Command cmd;
                cmd.cmd_name = String("get_id_board");
                cmdName = String("get_id_board");

                // 检查是否包含id字段并提取
                if (doc.containsKey("id"))
                {
                    cmd.id = doc["id"].as<String>();
                    if (print_key)
                    {
                        Serial.printf("接收到ID参数: %s\n", cmd.id.c_str());
                    }
                }
                else
                {
                    cmd.id = ""; // 如果没有提供id，设为空字符串
                }

                if (xQueueSend(commandQueue, &cmd, portMAX_DELAY) != pdPASS)
                {
                    Serial.println("命令队列已满，无法添加新命令");
                }
            }
            else if (doc["cmd_name"].as<String>() == "clock_ticking")
            {
                Command cmd;
                cmd.cmd_name = String("clock_ticking");
                cmdName = String("clock_ticking");

                // 检查是否包含id字段并提取
                if (doc.containsKey("id"))
                {
                    cmd.id = doc["id"].as<String>();
                    if (print_key)
                    {
                        Serial.printf("接收到ID参数: %s\n", cmd.id.c_str());
                    }
                }
                else
                {
                    cmd.id = ""; // 如果没有提供id，设为空字符串
                }

                if (xQueueSend(commandQueue, &cmd, portMAX_DELAY) != pdPASS)
                {
                    Serial.println("命令队列已满，无法添加新命令");
                }
            }
            else if (doc["cmd_name"].as<String>() == "illegal_move")
            {
                Command cmd;
                cmd.cmd_name = String("illegal_move");
                cmdName = String("illegal_move");

                // 检查是否包含id字段并提取
                if (doc.containsKey("id"))
                {
                    cmd.id = doc["id"].as<String>();
                    if (print_key)
                    {
                        Serial.printf("接收到ID参数: %s\n", cmd.id.c_str());
                    }
                }
                else
                {
                    cmd.id = ""; // 如果没有提供id，设为空字符串
                }

                if (xQueueSend(commandQueue, &cmd, portMAX_DELAY) != pdPASS)
                {
                    Serial.println("命令队列已满，无法添加新命令");
                }
            }

            // 以下是测试代码，用于处理未知命令
            //            else {
            //                // 处理未知命令
            //                StaticJsonDocument<512> response;
            //                response["cmd_name"] = doc["cmd_name"].as<String>();
            //                response["status"] = "failure";
            //                response["err"] = "Unknown command";
            //
            //                String responseStr;
            //                serializeJson(response, responseStr);
            //
            //                if (deviceConnected_g) {
            //                    pCharacteristic->setValue(responseStr.c_str());
            //                    pCharacteristic->notify();
            //                }
            //
            //                Serial.println("发送响应:");
            //                Serial.println(responseStr);
            //            }
        }
    }

    void onRead(BLECharacteristic *pCharacteristic)
    {
        // 客户端读取事件回调函数
        pCharacteristic->setValue(current_rev_json_cmd.c_str());
        if (print_key)
        {
            Serial.println("json_cmd@ " + current_rev_json_cmd);
        }
    }
};

///______________________________________________________________________________________________
/// fileio Service
String fileio_ascii_cmd = "";
class callback_fileio_ascii : public BLECharacteristicCallbacks
{
    void onWrite(BLECharacteristic *pCharacteristic)
    {
        std::string rxValue = pCharacteristic->getValue();
        if (rxValue.length() > 0)
        {
            Serial.printf("Received cmd: %s\n", rxValue.c_str());
            String jsonString = String(rxValue.c_str());

        } // rxValue.length() > 0
    } // onWrite

    void onRead(BLECharacteristic *pCharacteristic)
    { // 客户端读取事件回调函数
        pCharac_fileio_ascii->setValue(fileio_ascii_cmd.c_str());
        Serial.println("fileio@ " + fileio_ascii_cmd);
    }
};

String fileio_hex_cmd = "";
class callback_fileio_hex : public BLECharacteristicCallbacks
{
    void onWrite(BLECharacteristic *pCharacteristic)
    {

        std::string rxValue = pCharacteristic->getValue();
        if (rxValue.length() > 0)
        {
            Serial.printf("Received cmd: %s\n", rxValue.c_str());
            String jsonString = String(rxValue.c_str());

        } // rxValue.length() > 0
    } // onWrite

    void onRead(BLECharacteristic *pCharacteristic)
    { // 客户端读取事件回调函数
        pCharac_fileio_ascii->setValue(fileio_ascii_cmd.c_str());
        Serial.println("fileio@ " + fileio_ascii_cmd);
    }
};

///______________________________________________________________________________________________
/// debug Service
String debug_cmd = "debug";
class callback_debug_cmd : public BLECharacteristicCallbacks
{
    void onWrite(BLECharacteristic *pCharacteristic)
    {
        std::string rxValue = pCharacteristic->getValue();
        if (rxValue.length() > 0)
        {
            //            Serial.printf("Received cmd: %s\n", rxValue.c_str());
            debug_cmd = rxValue.c_str();

            // 将rxValue按分隔符','分割成多个字符串, 存储在cmds数组, 段数存为cmds_cn
            std::string cmds[10];
            std::string delimiter = ",";
            size_t pos = 0;
            int cmds_cnt = 0;

            while ((pos = rxValue.find(delimiter)) != std::string::npos)
            {
                cmds[cmds_cnt++] = rxValue.substr(0, pos);
                rxValue.erase(0, pos + delimiter.length());
            }

            cmds[cmds_cnt++] = rxValue;

            // 输出分割后的字符串
            //            for (int i = 0; i < cmds_cnt; i++) {
            //                Serial.printf("cmds[%d] = %s\n", i, cmds[i].c_str());
            //            }

            ////单命令模式
            if (cmds_cnt == 1)
            {
                if ((cmds[0] == "1") or (cmds[0] == "play"))
                {
                    if (print_key)
                    {
                        Serial.printf("play\n");
                    }
                }
                else if (toLowerCase(String(cmds[0].c_str())) == "gs")
                {
                    if (print_key)
                    {
                        Serial.printf("GlobalSteps: (%ld, %ld), Inside: (%ld, %ld)\n", int_gotoposition[0], int_gotoposition[1],
                                      Xaxis.currentPosition(), Yaxis.currentPosition());
                    }
                }
                else if (toLowerCase(String(cmds[0].c_str())) == "t1")
                {
                    if (print_key)
                    {
                        Serial.printf("t1\n");
                    }
                } // t1
                else if (toLowerCase(String(cmds[0].c_str())) == "t2")
                {

                } // t2
                else if (toLowerCase(String(cmds[0].c_str())) == "d_c")
                {
                    drawSquare(5, 5, 300);
                } // t2
            }
            else if (cmds_cnt == 2)
            { ////命令+数据模式 ____________________________________________

                if (toLowerCase(String(cmds[0].c_str())) == "speed")
                { /// 🚧 实时调速, 内置几种档位
                }
                else if (toLowerCase(String(cmds[0].c_str())) == "debug")
                { /// debug
                    /**测试
                     * \n
                     * tab1,1
                     * /1.1
                     * ///1.1
                     * #1.1
                     * */

                    //                     bool check = has_valid_cordinate();
                    //                     Serial.printf("[%s]check: %d\n", cmds[1].c_str(), check);
                }
            }
            else if (cmds_cnt == 3)
            { ////命令+数据模式
                if (toLowerCase(String(cmds[0].c_str())) == "gsset")
                { // 仅设置
                    long x = atoi(cmds[1].c_str());
                    long y = atoi(cmds[2].c_str());

                    int_gotoposition[0] = x;
                    int_gotoposition[1] = y;
                    MOTOR_RESET(int_gotoposition[0], int_gotoposition[1]);
                    if (print_key)
                    {
                        Serial.printf("GlobalSteps: %ld, %ld\n", int_gotoposition[0], int_gotoposition[1]);
                    }
                }
                else if (toLowerCase(String(cmds[0].c_str())) == "gs")
                { // 
                    long x = atoi(cmds[1].c_str());
                    long y = atoi(cmds[2].c_str());

                    //// 2:2 构型 相对
                    int_gotoposition[0] = x;
                    int_gotoposition[1] = y;
                    //______________________________________________
                    steppersControl.moveTo(int_gotoposition);
                    steppersControl.runSpeedToPosition();
                    if (print_key)
                    {
                        Serial.printf("GlobalSteps: %ld, %ld\n", int_gotoposition[0], int_gotoposition[1]);
                    }
                }

                else if (toLowerCase(String(cmds[0].c_str())) == "deg")
                { // 
                    if (print_key)
                    {
                        Serial.printf("MoveTo degree(");
                    }

                    float x_ = atof(cmds[1].c_str());
                    float y_ = atof(cmds[2].c_str());

                    Serial.print(x_);
                    if (print_key)
                    {
                        Serial.printf(",");
                    }
                    Serial.print(y_);
                    if (print_key)
                    {
                        Serial.printf(")\n");
                    }

                    //                    Serial.printf("GlobalSteps: %ld, %ld\n", int_gotoposition[0], int_gotoposition[1]);
                }
                else if (toLowerCase(String(cmds[0].c_str())) == "hbot")
                { // 
                    // hbot,200,200

                    MOTOR_ENABLE;
                    float x_ = atof(cmds[1].c_str());
                    float y_ = atof(cmds[2].c_str());
                    float delta_x = x_ - hbot_x;
                    float delta_y = y_ - hbot_y;
                    hbot_x = x_;
                    hbot_y = y_;

                    Serial.printf("hbot MoveTo (");
                    Serial.print(x_);
                    Serial.printf(",");
                    Serial.print(y_);
                    Serial.printf("), with delta (");

                    Serial.print(delta_x);
                    Serial.printf(",");
                    Serial.print(delta_y);
                    Serial.printf(")\n");

                    long tmp_hbot_position[2];
                    // 判断delta_x，delta_y在笛卡尔坐标的哪个象限
                    if (delta_x >= 0 && delta_y >= 0)
                    {
                        if (print_key)
                        {
                            Serial.printf("The vector is in the first quadrant.\n");
                        }
                        MotorSteps s1 = calc_vector_x(delta_x);
                        MotorSteps s2 = calc_vector_y(delta_y);
                        tmp_hbot_position[0] = s1.m1_step_value + s2.m1_step_value;
                        tmp_hbot_position[1] = s1.m2_step_value + s2.m2_step_value;

                        if (print_key)
                        {
                            Serial.printf("local: %ld, %ld\n", tmp_hbot_position[0], tmp_hbot_position[1]);
                        }
                    }
                    else if (delta_x < 0 && delta_y >= 0)
                    {
                        if (print_key)
                        {
                            Serial.printf("The vector is in the second quadrant.\n");
                        }
                        MotorSteps s1 = calc_vector_x(delta_x);
                        MotorSteps s2 = calc_vector_y(delta_y);
                        tmp_hbot_position[0] = s1.m1_step_value + s2.m1_step_value;
                        tmp_hbot_position[1] = s1.m2_step_value + s2.m2_step_value;
                        if (print_key)
                        {
                            Serial.printf("local: %ld, %ld\n", tmp_hbot_position[0], tmp_hbot_position[1]);
                        }
                    }
                    else if (delta_x < 0 && delta_y < 0)
                    {
                        if (print_key)
                        {
                            Serial.printf("The vector is in the third quadrant.\n");
                        }
                        MotorSteps s1 = calc_vector_x(delta_x);
                        MotorSteps s2 = calc_vector_y(delta_y);
                        tmp_hbot_position[0] = s1.m1_step_value + s2.m1_step_value;
                        tmp_hbot_position[1] = s1.m2_step_value + s2.m2_step_value;
                        if (print_key)
                        {
                            Serial.printf("local: %ld, %ld\n", tmp_hbot_position[0], tmp_hbot_position[1]);
                        }
                    }
                    else if (delta_x >= 0 && delta_y < 0)
                    {
                        if (print_key)
                        {
                            Serial.printf("The vector is in the fourth quadrant.\n");
                        }
                        MotorSteps s1 = calc_vector_x(delta_x);
                        MotorSteps s2 = calc_vector_y(delta_y);
                        tmp_hbot_position[0] = s1.m1_step_value + s2.m1_step_value;
                        tmp_hbot_position[1] = s1.m2_step_value + s2.m2_step_value;
                        if (print_key)
                        {
                            Serial.printf("local: %ld, %ld\n", tmp_hbot_position[0], tmp_hbot_position[1]);
                        }
                    }

                    hbot_position[0] += tmp_hbot_position[0];
                    hbot_position[1] += tmp_hbot_position[1];

                    if (print_key)
                    {
                        Serial.printf("Hbot: %ld, %ld\n", hbot_position[0], hbot_position[1]);
                    }
                    steppersControl.moveTo(hbot_position);
                    steppersControl.runSpeedToPosition();

                    MOTOR_DISABLE;
                }

                else if (toLowerCase(String(cmds[0].c_str())) == "hbots")
                { // ✅
                    MOTOR_ENABLE;
                    float x_ = atof(cmds[1].c_str());
                    float y_ = atof(cmds[2].c_str());
                    float delta_x = x_ - hbot_x;
                    float delta_y = y_ - hbot_y;
                    hbot_x = x_;
                    hbot_y = y_;

                    Serial.printf("hbot MoveTo (");
                    Serial.print(x_);
                    Serial.printf(",");
                    Serial.print(y_);
                    Serial.printf("), with delta (");

                    Serial.print(delta_x);
                    Serial.printf(",");
                    Serial.print(delta_y);
                    Serial.printf(")\n");

                    long tmp_hbot_position[2];
                    // 判断delta_x，delta_y在笛卡尔坐标的哪个象限
                    if (delta_x >= 0 && delta_y >= 0)
                    {
                        //                        Serial.printf("The vector is in the first quadrant.\n");
                        MotorSteps s1 = calc_vector_x(delta_x);
                        MotorSteps s2 = calc_vector_y(delta_y);
                        tmp_hbot_position[0] = s1.m1_step_value + s2.m1_step_value;
                        tmp_hbot_position[1] = s1.m2_step_value + s2.m2_step_value;

                        //                        Serial.printf("local: %ld, %ld\n", tmp_hbot_position[0], tmp_hbot_position[1]);
                    }
                    else if (delta_x < 0 && delta_y >= 0)
                    {
                        //                        Serial.printf("The vector is in the second quadrant.\n");
                        MotorSteps s1 = calc_vector_x(delta_x);
                        MotorSteps s2 = calc_vector_y(delta_y);
                        tmp_hbot_position[0] = s1.m1_step_value + s2.m1_step_value;
                        tmp_hbot_position[1] = s1.m2_step_value + s2.m2_step_value;
                        //                        Serial.printf("local: %ld, %ld\n", tmp_hbot_position[0], tmp_hbot_position[1]);
                    }
                    else if (delta_x < 0 && delta_y < 0)
                    {
                        //                        Serial.printf("The vector is in the third quadrant.\n");
                        MotorSteps s1 = calc_vector_x(delta_x);
                        MotorSteps s2 = calc_vector_y(delta_y);
                        tmp_hbot_position[0] = s1.m1_step_value + s2.m1_step_value;
                        tmp_hbot_position[1] = s1.m2_step_value + s2.m2_step_value;
                        //                        Serial.printf("local: %ld, %ld\n", tmp_hbot_position[0], tmp_hbot_position[1]);
                    }
                    else if (delta_x >= 0 && delta_y < 0)
                    {
                        //                        Serial.printf("The vector is in the fourth quadrant.\n");
                        MotorSteps s1 = calc_vector_x(delta_x);
                        MotorSteps s2 = calc_vector_y(delta_y);
                        tmp_hbot_position[0] = s1.m1_step_value + s2.m1_step_value;
                        tmp_hbot_position[1] = s1.m2_step_value + s2.m2_step_value;
                        //                        Serial.printf("local: %ld, %ld\n", tmp_hbot_position[0], tmp_hbot_position[1]);
                    }

                    long last_hbot_position[2] = {0};
                    last_hbot_position[0] = hbot_position[0];
                    last_hbot_position[1] = hbot_position[1];

                    hbot_position[0] += tmp_hbot_position[0];
                    hbot_position[1] += tmp_hbot_position[1];

                    if (print_key)
                    {
                        Serial.printf("Hbot: %ld, %ld\n", hbot_position[0], hbot_position[1]);
                    }

                    for (int i = 0; i < 10; i++)
                    {
                        steppersControl.moveTo(hbot_position);
                        steppersControl.runSpeedToPosition();
                        // delay(100);
                        steppersControl.moveTo(last_hbot_position);
                        steppersControl.runSpeedToPosition();
                    }

                    MOTOR_DISABLE;
                }
                else if (toLowerCase(String(cmds[0].c_str())) == "move")
                {
                    //
                    Command cmd;
                    cmd.cmd_name = "move_debug";
                    cmd.startPos = String(cmds[1].c_str());
                    cmd.endPos = String(cmds[2].c_str());
                    if (xQueueSend(commandQueue, &cmd, portMAX_DELAY) != pdPASS)
                    {
                        Serial.println("命令队列已满，无法添加新命令");
                    }
                }

            } // cmds_cnt==N

        } // rxValue.length() > 0
    } // onWrite

    void onRead(BLECharacteristic *pCharacteristic)
    { // 客户端读取事件回调函数
        pCharac_debug_cmd->setValue(debug_cmd.c_str());
        if (print_key)
        {
            Serial.println("debug@ " + debug_cmd);
        }
    }
};

///______________________________________________________________________________________________

/**
 * ble_printf_init
 * 还需要配合使用
 * pService_tx->start();;
 * */

BLECharacteristic *pCharac_uart_tx; // 定义UART传输特性

/// BLE_UART_Service
void btprintf(const char *fmt, ...)
{
    char buf[256];
    va_list args;
    va_start(args, fmt);
    vsnprintf(buf, sizeof(buf), fmt, args);
    va_end(args);
    pCharac_uart_tx->setValue(buf);
    if (deviceConnected_g)
    {
        pCharac_uart_tx->notify();
        if (print_key)
        {
            Serial.printf("TX : %s", buf);
        }
    }
}

void btprintf2(const char *fmt, ...)
{
    char buf[256];
    va_list args;
    va_start(args, fmt);
    vsnprintf(buf, 256, fmt, args);
    va_end(args);

    int len = strlen(buf);
    int len_sent = 0;

    while (len_sent < len)
    {
        int send_size = (len - len_sent > MAX_BTBUF_SIZE) ? MAX_BTBUF_SIZE : len - len_sent;
        char send_buf[MAX_BTBUF_SIZE + 1] = {0};
        memcpy(send_buf, &buf[len_sent], send_size);

        pCharac_uart_tx->setValue(send_buf);

        if (deviceConnected_g)
        {
            pCharac_uart_tx->notify();
        }

        len_sent += send_size;
    }
}

///______________________________________________________________________________________________
//// 蓝牙类
Bluetooth::Bluetooth()
{
}

// 包含service，characteristic初始化；关联回调函数
int Bluetooth::init(String name)
{
    // Create the BLE Device
    BLEDevice::init(name.c_str());

    //// Create the BLE Server
    pServer = BLEDevice::createServer();                 // 创建服务器
    pServer->setCallbacks(new BleMainServerCallbacks()); // 绑定回调函数

    ///[1]______________________________________________________________________________________________
    BLEService *pService_notify = pServer->createService(SERVICE_NOTIFY_UUID);
    //___________________________________
    pCharac_notify_msg = pService_notify->createCharacteristic(
        CHARAC_NOTIFY_MSG_UUID,
        BLECharacteristic::PROPERTY_READ |
            BLECharacteristic::PROPERTY_NOTIFY);
    pCharac_notify_msg->addDescriptor(new BLE2902()); // notify必需
    pCharac_notify_msg->setValue("null");             // 读取的默认值

    pService_notify->start();
    ///[2]______________________________________________________________________________________________
    BLEService *pCharac_state = pServer->createService(STATE_NOTIFY_UUID);
    //___________________________________
    pCharac_state_msg = pCharac_state->createCharacteristic(
        STATE_NOTIFY_UUID,
        BLECharacteristic::PROPERTY_READ |
            BLECharacteristic::PROPERTY_NOTIFY);
    pCharac_state_msg->addDescriptor(new BLE2902()); // notify必需
    pCharac_state_msg->setValue("null");             // 读取的默认值

    pCharac_state->start();
    ///[5]______________________________________________________________________________________________
    BLEService *pService_json = pServer->createService(SERVICE_JSON_UUID);
    //___________________________________
    pCharac_json = pService_json->createCharacteristic(
        CHARAC_JSON_UUID,
        BLECharacteristic::PROPERTY_READ |
            BLECharacteristic::PROPERTY_WRITE |
            BLECharacteristic::PROPERTY_NOTIFY

    );
    pCharac_json->addDescriptor(new BLE2902()); // notify必需
    pCharac_json->setValue(json_cmd.c_str());   // 设置该Characteristic的Value值
    pCharac_json->setCallbacks(new callback_json_cmd());

    pCharacteristic = pCharac_json;
    pService_json->start();
    ///[6]______________________________________________________________________________________________
    BLEService *pService_fileio = pServer->createService(SERVICE_FILEIO_UUID);
    //___________________________________
    pCharac_fileio_ascii = pService_fileio->createCharacteristic(
        CHARAC_FILEIO_ASCII_UUID,
        BLECharacteristic::PROPERTY_READ |
            BLECharacteristic::PROPERTY_WRITE);
    pCharac_fileio_ascii->setValue(json_cmd.c_str()); // 设置该Characteristic的Value值
    pCharac_fileio_ascii->setCallbacks(new callback_fileio_ascii());
    //___________________________________
    pCharac_fileio_hex = pService_fileio->createCharacteristic(
        CHARAC_FILEIO_HEX_UUID,
        BLECharacteristic::PROPERTY_READ |
            BLECharacteristic::PROPERTY_WRITE);
    pCharac_fileio_hex->setValue(json_cmd.c_str()); // 设置该Characteristic的Value值
    pCharac_fileio_hex->setCallbacks(new callback_fileio_hex());

    pService_fileio->start();
    ///[7]______________________________________________________________________________________________
    BLEService *pService_debug = pServer->createService(SERVICE_DEBUG_UUID);
    //___________________________________
    pCharac_debug_cmd = pService_debug->createCharacteristic(
        CHARAC_DEBUG_UUID,
        BLECharacteristic::PROPERTY_READ |
            BLECharacteristic::PROPERTY_WRITE);
    pCharac_debug_cmd->setValue(json_cmd.c_str()); // 设置该Characteristic的Value值
    pCharac_debug_cmd->setCallbacks(new callback_debug_cmd());
    pService_debug->start();
    ///______________________________________________________________________________________________
    //// Create the BLE UART Service
    BLEService *pService_tx = pServer->createService(SERVICE_UART_UUID);
    pCharac_uart_tx = pService_tx->createCharacteristic( // 创建特征
        CHARAC_UART_TX_UUID,
        BLECharacteristic::PROPERTY_NOTIFY // 启用通知
    );
    pCharac_uart_tx->addDescriptor(new BLE2902()); // 不加这行可能无法使用通知

    pService_tx->start();
    ///______________________________________________________________________________________________
    BLEAdvertising *pAdvertising = BLEDevice::getAdvertising(); // 获取Advertising对象
    // 广播添加服务 UUID
    pAdvertising->addServiceUUID(SERVICE_JSON_UUID);
    pAdvertising->addServiceUUID(SERVICE_FILEIO_UUID);
    pAdvertising->addServiceUUID(SERVICE_NOTIFY_UUID);
    pAdvertising->addServiceUUID(SERVICE_DEBUG_UUID);
    pAdvertising->addServiceUUID(SERVICE_UART_UUID);

    //___________________________________
    pAdvertising->setScanResponse(true); // active scan uses more power, but get results faster
    pAdvertising->setMinPreferred(0x06); // functions that help with iPhone connections issue
    pAdvertising->setMinPreferred(0x12);
    BLEDevice::startAdvertising();
    return 0;
}

void Bluetooth::setName(String name)
{
    // 实现重新设置ESP32 BLE 设备名称的代码
    if (print_key)
    {
        Serial.print("设置蓝牙名称为: ");
        Serial.println(name.c_str());
    }

    // 停止广播
    BLEDevice::getAdvertising()->stop();

    // 不再使用deinit/init的方式，直接设置设备名称并重启广播
    esp_err_t ret = esp_ble_gap_set_device_name(name.c_str());
    if (ret != ESP_OK)
    {
        if (print_key)
        {
            Serial.printf("设置蓝牙名称失败，错误码: %d\n", ret);
        }
    }

    // 清除之前的设备并重新启动广播
    BLEDevice::getAdvertising()->start();
    if (print_key)
    {
        Serial.println("重新开始广播");
    }
}
